#ifndef AGV_COMMUNICATION__MODBUS_CLIENT_HPP_
#define AGV_COMMUNICATION__MODBUS_CLIENT_HPP_
#include <memory>
#include <iostream>
#include <mutex>
#include <vector>
#include <queue>
#include <modbus/modbus.h>

class ModbusClient
{
private:
    modbus_t *ctx_;
    const char *ip_;
    int port_;
    bool connected_ = false;
    std::mutex modbus_mutex_;

public:
    using Ptr = std::shared_ptr<ModbusClient>;

    ModbusClient(const char *ip, int port)
        : ip_(ip), port_(port)
    {
    }

    int initClient()
    {
        // 创建 Modbus TCP 连接
        ctx_ = modbus_new_tcp(ip_, port_);
        if (ctx_ == nullptr)
        {
            std::cerr << "Unable to create Modbus context\n";
            connected_ = false;
            return -1;
        }

        // 连接服务器
        if (modbus_connect(ctx_) == -1)
        {
            std::cerr << "Connection failed: " << modbus_strerror(errno) << "\n";
            modbus_free(ctx_);
            connected_ = false;
            return -1;
        }

        connected_ = true;
        std::cout << "Connected to Modbus server at " << ip_ << ":" << port_ << "\n";
        return 1;
    }

    ~ModbusClient() {}

    void close()
    {
        modbus_close(ctx_);
    }

    void free()
    {
        modbus_free(ctx_);
    }

    bool read(int addr, int num, uint16_t *ret)
    {
        std::lock_guard<std::mutex> lock(modbus_mutex_);
        // 读取保持寄存器（例如地址 0，读取 5 个寄存器）
        int rc = modbus_read_registers(ctx_, addr, num, ret);
        if (rc == -1)
        {
            std::cerr << "Failed to read: " << modbus_strerror(errno) << "\n";
            return false;
        }
        else
        {
            return true;
        }
    }

    bool write(int addr, uint16_t value)
    {
        std::lock_guard<std::mutex> lock(modbus_mutex_);
        // 写单个保持寄存器（例如地址 0，写入 123）
        int rc = modbus_write_register(ctx_, addr, value);
        if (rc == -1)
        {
            std::cerr << "Failed to write: " << modbus_strerror(errno) << "\n";
            return false;
        }
        else
        {
            std::cout << "Successfully wrote value to register 0\n";
            return true;
        }
    }

    bool writeBuff(int addr, int num, uint16_t* value)
    {
        std::lock_guard<std::mutex> lock(modbus_mutex_);
        // 写单个保持寄存器（例如地址 0，写入 123）(modbus_t *ctx, int addr, int nb, const uint16_t *data
        int rc = modbus_write_registers(ctx_, addr, num, value);
        if (rc == -1)
        {
            std::cerr << "Failed to write: " << modbus_strerror(errno) << "\n";
            return false;
        }
        else
        {
            std::cout << "Successfully wrote value to register 0\n";
            return true;
        } 
    }

    bool isConnected()
    {
        return connected_;
    }
};

#endif
